/**********************************************************************
 Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved.
***********************************************************************/

#include "unitree_legged_sdk/unitree_legged_sdk.h"
#include <math.h>
#include <iostream>
#include <unistd.h>
#include <string.h>

using namespace UNITREE_LEGGED_SDK;

#define ALL_TEST 0
#define WALK_TEST 1

class Custom {
 public:
  Custom(uint8_t level) : safe(LeggedType::Go1),
                          udp(level, 8090, "192.168.123.161", 8082) {
    udp.InitCmdData(cmd);
  }
  void UDPRecv();
  void UDPSend();
  void RobotControl();
  void RobotWalk();
  void RobotForward();
  void RobotLeft();
  void RobotRight();
  void RobotBackward();
  void static dogStartTest(int type);

  Safety safe;
  UDP udp;
  HighCmd cmd = {0};
  HighState state = {0};
  int motiontime = 0;
  float dt = 0.002;  // 0.001~0.01
  static std::string forward;
  static std::string left;
  static std::string right;
  static std::string backward;

};